#include <iostream>
#include <vector>
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/video/tracking.hpp>
#include <opencv2/core/core.hpp>
//#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv/cv.h>
#include <iostream>
#include <sstream>
#include <string> 
#include <opencv2/video/tracking.hpp>

using namespace cv;
using namespace std;


int soglia=170;
void colorfilter(const Mat frame,Mat& mask);
void buildTrackbar();

Mat mask;

struct mouse_info_struct { int x,y; };
struct mouse_info_struct mouse_info = {-1,-1}, last_mouse;

void on_mouse(int event, int x, int y, int flags, void* param) {
	//if (event == CV_EVENT_LBUTTONUP) 
	{
		last_mouse = mouse_info;
		mouse_info.x = x;
		mouse_info.y = y;
		
//		cout << "got mouse " << x <<","<< y <<endl;
	}
}


int main988()
{
		
	return 1;
}





int mainMyKalman()
{

	
	// KALMAN FILTER
	double errorP=0.1;
	double errorR=0.01;
	double errorQ=0.01;

	Mat A = (Mat_<float>(4,4)<<1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); 

	Mat P(4,4,CV_32F);
	Mat R(4,4,CV_32F);
	Mat Q(4,4,CV_32F);
	Mat H(4,4,CV_32F);
	Mat I(4,4,CV_32F);
	setIdentity(I);
	setIdentity(H);
	setIdentity(P);
	setIdentity(Q);
	setIdentity(R);

	P=P*errorP;
	R=R*errorR;
	Q=Q*errorQ;

	cout<<P<<endl;
	 

	Mat X= (Mat_<float>(4,1)<<0,0,0,0);

	Mat Z = (Mat_<float>(4,1)<<0,0,0,0); 


	/*KalmanFilter kf=KalmanFilter(4,2,0);
	kf.errorCovPost=P;
	kf.errorCovPre=P;
	kf.transitionMatrix = A;
	kf.measurementMatrix = H;*/

	//end settings


	buildTrackbar();

	Mat frame;
	VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\ball.avi");
	

	while(1)
	{
	vc>>frame;
	if(!frame.data)
		break;

	imshow("Frame",frame);
	waitKey(30);
	
	colorfilter(frame,mask);

	Moments m = moments(mask,true);
	Point center ((m.m10/m.m00),(m.m01/m.m00));

	Mat zero = Mat::zeros(mask.rows,mask.cols,CV_8U);


	

	vector<Mat> arrayMat;
	arrayMat.push_back(mask);
	arrayMat.push_back(zero);
	arrayMat.push_back(zero);
	merge(arrayMat,mask);

	circle(mask,center,1,Scalar(0,255,0),2);
	

	Mat Y,S;
	Mat Ht,K,At;
	transpose(H,Ht);
	transpose(A,At);

	Z = (Mat_<float>(4,1)<<center.x,center.y,abs(Z.at<float>(2)-center.x),abs(Z.at<float>(3)-center.y)); 
	//Z = (Mat_<float>(4,1)<<center.x,center.y,0,0); 
	cout<<Z<<endl;

	X=A*X;


	P=A*P*At+Q;

	
	
	//update
	Y=Z-H*X;	
	S=H*P*Ht+R;
	K=P*H*S;
	X=X+(K*Y);
	P=(I-K*H)*P;
	

	/*kf.statePre.at<float>(0)=center.x;
	kf.statePre.at<float>(1)=center.y;
	kf.statePre.at<float>(2)=0;
	kf.statePre.at<float>(3)=0;*/

	cout<<X<<endl;
	//system("pause");
 
	circle(mask,Point(X.at<float>(0),X.at<float>(1)),1,Scalar(0,0,255),2);


	imshow("Mask1",mask);
	waitKey(30);



	}

	return 1;
}

void colorfilter(const Mat frame,Mat& mask)
{
	//Mat hsv;
	//cvtColor(frame,hsv,CV_BGR2HSV);

	vector<Mat>level;
	split(frame,level);
	
	//value
	
	frame.copyTo(mask);

	 
	inRange(mask,Scalar(soglia-20,0,0),Scalar(soglia+20,0,0),mask);
	

}

void buildTrackbar()
{
	namedWindow("Trackbar");
	createTrackbar("Value: ","Trackbar",&soglia,256);
}
